/*! \file	monoslam.hpp
 *  \brief	Declarations for the MONOSLAM node.
*/

#ifndef _THERMALVIS_MONOSLAM_H_
#define _THERMALVIS_MONOSLAM_H_
	
#include "general_resources.hpp"
#include "ros_resources.hpp"
#include "opencv_resources.hpp"
#include "pcl_resources.hpp"

#include "improc.hpp"
#include "video.hpp"
#include "features.hpp"
#include "reconstruction.hpp"
#include "sba.hpp"
#include "keyframes.hpp"
#include "geometry.hpp"

#include "feature_tracks.h"

#include "monoslamConfig.h"

typedef dynamic_reconfigure::Server < thermalvis::monoslamConfig > Server;

// Drawing
#define DEFAULT_DRAWGRAPH_DECIMATION			1
#define DEFAULT_DRAWGRAPH_BICOLOR				0

// Hard Limits
#define MAX_FRAMES								1000
#define MAX_TRACKS 								10000
#define SBA_MEMORY								134217728
#define MAX_SEGMENTS							16
#define INITIALIZATION_SCORING_PARAMETERS		5

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

const char __PROGRAM__[] = "THERMALVIS_MONOSLAM";

bool wantsToShutdown = false;
void mySigintHandler(int sig);

/// \brief		Stores configuration information for the SLAM routine
struct slamData {
	
	string stream;
	
	double motionThreshold;
	int adjustmentFrames;
	int flowback;
	double minStartupScore;
	cameraParameters cameraData;
	bool verboseMode;
	bool obtainStartingData(ros::NodeHandle& nh);
	
	double dataTimeout;
	
	int keyframeSpacing;
	
	int maxGuides;
	int maxKeyframeSeparation;
	
	int framesForTriangulation;
	
	int timeSpacing;
	
	int minTrackedFeatures;
	
	int min3dPtsForPnP;
	
	int maxInitializationFrames;

	
	double minGeometryScore;
	double minKeyframeScore;
	
	double requiredTrackFrac;
	
	int camerasPerSys;
	
	bool timeDebug;
	
	bool logErrors;
	
	int maxTestsPerFrame;
	
	int initialStructureIterations, poseEstimateIterations, fullSystemIterations, subsequenceIterations, keyframeIterations;
	
	bool keyframeEvaluationMode; 
	
	int minStartingSeparation, maxStartingSeparation;
	
	string read_addr, evaluationFile, errorFile, initializationScorecard;
	
};

/// \brief		Used to analyze chokepoints in processing time
struct timeAnalyzer {
	
	int cycles;
	double vals[1024];
	double average;
	double sigma;
	
	struct timeval cycle_timer;
	
	timeAnalyzer();
	void calcParameters();
	void startRecording();
	void stopRecording();
	
};

/// \brief		Manages the SLAM procedure
class slamNode {
private:

	int decimation;
	int bicolor;

	bool isTracking;
	
	bool f1;
	
	bool evaluationCompleted;

	unsigned int baseConnectionNum;
	
	double **scorecardParams;

	slamData configData;
	
	ofstream evaluationStream;
	
	unsigned int nextFrame;
	
	unsigned int putativelyEstimatedFrames;
	
	cv::Mat eye4;
	
	bool repetitionNoted;
	
	ofstream error_file;
	char error_filename[256];

	ros::Subscriber tracks_sub;
	ros::Subscriber info_sub;
	
	SysSBA sys, display_sys;
	ros::Publisher path_pub;
	ros::Publisher camera_pub;
	ros::Publisher points_pub;
	
	ros::Timer timer;
	
	bool firstIteration;
	
	cv::Mat blank;
	
	bool structureValid, structureFormed;
	
	int latestFrame;
	
	bool infoProcessed;
	
	struct timeval cycle_timer;
	double elapsedTime;
	
	double distanceConstraint;
	
	vector<featureTrack> featureTrackVector;
	
	cv::Point3d cloudCentroid[MAX_SEGMENTS], cloudStdDeviation[MAX_SEGMENTS];
	keyframeStore keyframe_store;
	cv::Mat F_arr[MAX_FRAMES], H_arr[MAX_FRAMES];
	
	cv::Mat ACM[MAX_FRAMES], ACM2[MAX_FRAMES];
	
	cv::Mat keyframeTestScores, keyframeTestFlags;
	
	char nodeName[256];
	
	ros::Publisher pose_pub;
	char pose_pub_name[256];
	
	geometry_msgs::PoseStamped currentPose;
	
	int currentPoseIndex;
	
	double minimumKeyframeScore;
	
	vector<unsigned int> framesReceived;
	
	unsigned int lastBasePose;
	
	unsigned int startingTracksCount;
	
	timeAnalyzer trackHandlingTime;
	timeAnalyzer triangulationTime;
	timeAnalyzer poseEstimationTime;
	timeAnalyzer bundleAdjustmentTime;

	// Thread-protection
	boost::mutex cam_mutex;
	boost::mutex tracks_mutex;
	boost::mutex keyframes_mutex;
	boost::mutex main_mutex;
	
	dynamic_reconfigure::Server<thermalvis::monoslamConfig> server;
	dynamic_reconfigure::Server<thermalvis::monoslamConfig>::CallbackType f;
	
public:

	slamNode(ros::NodeHandle& nh, slamData startupData);
	void assignPose(geometry_msgs::PoseStamped& pPose, cv::Mat& C);
	
	void estimatePose(vector<unsigned int>& basisNodes, unsigned int idx);
	
	void getBasisNodes(vector<unsigned int>& basisNodes, unsigned int idx);
	
	// Callbacks
	void handle_tracks(const thermalvis::feature_tracksConstPtr& msg);
	void handle_info(const sensor_msgs::CameraInfoConstPtr& info_msg);
	void main_loop(const ros::TimerEvent& event);
	
	// Routines
	bool formInitialStructure();
	bool findStartingFrames();
	void refreshPoses();
	bool checkForKeyframe();
	void processNextFrame();
	void update_display();
	void show_poses();
	void update_cameras_to_pnp();
	double assignStartingFrames(unsigned int best_iii, unsigned int best_jjj, double* keyframe_scores, cv::Mat& startingTrans);
	
	void getGuidingPose(cv::Mat *srcs, cv::Mat& dst, unsigned int idx);
	
	void clearSystem();
	
	void processScorecard();
	
	void serverCallback(thermalvis::monoslamConfig &config, uint32_t level);
	
	// Misc
	bool stillTracking() {
		return isTracking;
	}
	
	void prepareForTermination();
	
};

boost::shared_ptr < slamNode > *globalNodePtr;

#endif
